3D Pose Estimator #28
Conversation
|
Warning Rate limit exceeded
⌛ How to resolve this issue?After the wait time has elapsed, a review can be triggered using the We recommend that you space out your commits to avoid hitting the rate limit. 🚦 How do rate limits work?CodeRabbit enforces hourly rate limits for each developer per organization. Our paid plans have higher rate limits than the trial, open-source and free plans. In all cases, we re-allow further reviews after a brief timeout. Please see our FAQ for further information. WalkthroughThe PR migrates many pose-related APIs from 2D to 3D types (Pose3d/Transform3d/Rotation3d) across the codebase, updating RobotPoseEstimator, AprilTagCamera, RelativeRobotPoseSource, and related classes. RobotPoseEstimator now exposes Pose3d and provides 2D helpers (getEstimated2DRobotPose, sample2DPoseAtTimestamp). Call sites were adjusted to use 3D poses or the new 2D helpers. Gyro handling was extended to include roll and pitch. A submodule reference under src/main/java/frc/trigon/lib was updated to a new commit hash. 🚥 Pre-merge checks | ✅ 1 | ❌ 2❌ Failed checks (2 warnings)
✅ Passed checks (1 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 7
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (2)
src/main/java/frc/trigon/robot/subsystems/turret/Turret.java (1)
82-85: Reset turret target on stop to avoid stale commands.
stop()halts the motor but preservestargetSelfRelativeAngle, which can re-issue a stale target when re-enabled. Reset the target field (and any related closed-loop state) instop().Based on learnings, ensure all subsystem stop() methods reset target fields.
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)
28-41: Flatten camera-to-robot transform to match contract.The Javadoc (line 29) states "only the x, y and yaw values will be used," but line 40 applies the full 3D inverse, which includes z, roll, and pitch if present in the input. This violates the documented contract. If
robotCenterToCameracontains non-zero z/roll/pitch, they will propagate and skew the robot pose estimate.Flatten to 2D before inversion to enforce the contract:
Proposed fix
- this.cameraToRobotCenter = robotCenterToCamera.inverse(); + final Translation3d translation = robotCenterToCamera.getTranslation(); + final Rotation2d yaw = robotCenterToCamera.getRotation().toRotation2d(); + final Transform3d flattened = new Transform3d( + new Translation3d(translation.getX(), translation.getY(), 0.0), + new Rotation3d(0.0, 0.0, yaw.getRadians()) + ); + this.cameraToRobotCenter = flattened.inverse();
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
129-129: Same 3D reset concern as in AutonomousConstants
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
Outdated
Show resolved
Hide resolved
.../java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java
Outdated
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java
Outdated
Show resolved
Hide resolved
...rc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java
Show resolved
Hide resolved
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/main/java/frc/trigon/robot/subsystems/turret/Turret.java (1)
83-85: Reset target state in stop().Line 83 stops the motor but leaves
targetSelfRelativeAngleintact, so a subsequent enable can jump to a stale target. Reset the target to 0 instop(). Based on learnings, subsystem stop methods must clear target fields.Based on learnings, ensure stop resets target fields to 0.Proposed fix
public void stop() { masterMotor.stopMotor(); + targetSelfRelativeAngle = Rotation2d.fromDegrees(0); }
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
67-69: Resetting withnew Pose3d(resetPose2d)drops z/roll/pitch.Line 68 zeros z/roll/pitch. If the estimator tracks tilt, preserve existing z/roll/pitch when resetting from a 2D pose.
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java (1)
142-149: Add a null guard before calling getRotation().Line 142 can return null when the pose buffer has no sample for that timestamp, which will throw a NullPointerException and abort constrained solve. Guard the sample and return null early.
Proposed fix
-import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; @@ - final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds()).getRotation(); - final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp); + final Pose2d sampledPose = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds()); + if (sampledPose == null) + return null; + final Rotation2d robotYawAtTimestamp = sampledPose.getRotation(); + final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp);src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)
96-99: Avoid returning null fromcalculateRobotPose().
Returning null propagates intoestimatedRobotPose, makinggetEstimatedRobotPose()nullable and prone to NPEs for callers that do not gate onhasValidResult().Proposed fix
- if (!hasValidResult()) - return null; + if (!hasValidResult()) + return estimatedRobotPose;
There was a problem hiding this comment.
Actionable comments posted: 1
♻️ Duplicate comments (2)
src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (2)
310-316: Validate all gyro signal array lengths to preventArrayIndexOutOfBoundsException.The code assumes roll, pitch, and yaw arrays have equal length but only checks the first. If any signal buffer has fewer samples, the loop at lines 320-327 will throw.
Suggested fix
final double[][] odometryUpdatesDegrees = new double[][]{ gyro.getThreadedSignal(Pigeon2Signal.ROLL), gyro.getThreadedSignal(Pigeon2Signal.PITCH), gyro.getThreadedSignal(Pigeon2Signal.YAW) }; - final int totalOdometryUpdates = odometryUpdatesDegrees[0].length; + final int totalOdometryUpdates = Math.min( + Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length), + odometryUpdatesDegrees[2].length + );
329-329: Align timestamps with truncated odometry sample count.If
phoenix6SignalThread.getLatestTimestamps()returns more samples thantotalOdometryUpdates,updatePoseEstimatorOdometryreceives misaligned data. Include timestamps in the min calculation and slice to match.Suggested fix
+import java.util.Arrays; ... + final double[] latestTimestamps = phoenix6SignalThread.getLatestTimestamps(); final int totalOdometryUpdates = Math.min( - Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length), - odometryUpdatesDegrees[2].length + Math.min( + Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length), + odometryUpdatesDegrees[2].length + ), + latestTimestamps.length ); ... - RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry(swerveWheelPositions, gyroRotations, phoenix6SignalThread.getLatestTimestamps()); + RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry( + swerveWheelPositions, + gyroRotations, + Arrays.copyOf(latestTimestamps, totalOdometryUpdates) + );
No description provided.